#!/usr/bin/python
#coding:utf-8

# Copyright 2011 Nicolau Leal Werneck, Anna Helena Reali Costa and
# Universidade de São Paulo
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

###############################################################################
## Open a (pinhole camera model) picture, find its orientation and
## extract the rectified.
##
## Changes that must be performed as soon as possible: read the
## intrinsic parameters from somewhere, and make it easy to switch to
## the equirectangular model.

import sys

import time

import matplotlib

if __name__ == '__main__':
    if sys.argv[0][-7:] == '-nox.py':
        matplotlib.use('Agg') 

from pylab import *

import scipy.io

from numpy import dot

from camori import PicturePinhole, quaternion_to_matrix, measure_error, quaternion_product, dir_colors

import simplejson

import Image


if __name__ == '__main__':

    ## Plot stuff immediately
    ion()

    rc('text',usetex=False)

    ## Avoid zero divide warnins...
    np.seterr(divide='ignore')

    #Downscale = True
    Downscale = False

    #PlotStuff=False
    PlotStuff=True

    #################################################################
    ## Load image and initialize pic object

    ## Sets filename from input argument
    if len(sys.argv) < 3:
        print sys.argv[0], '<job_file.json> <frame_number>'
        raise Exception('Insufficient number of parameters')

    finput = open(sys.argv[1])
    job_params = simplejson.load(finput)
    finput.close()

    fileroot = job_params['root_directory']

    framenum = int(sys.argv[2])
    filename = fileroot+'/frames/'+job_params['filename_format']%framenum

    im = Image.open(filename)
    frame = array(im.convert('RGB'), dtype=float)
    imr = array(im.convert('RGB'), dtype=float)
    imr = imr[:,:,:3] #remove alpha channel

    # Smooth out
    if ("gaussian_smoothing_factor" in job_params.keys() and 
        job_params["gaussian_smoothing_factor"] > 0):
        for c in range(3):
            imr[:,:,c] = scipy.ndimage.gaussian_filter( imr[:,:,c], double(job_params["gaussian_smoothing_factor"]))

    ## Rescales image
    if Downscale:
        zzz = zeros( (imr.shape[0]/4,imr.shape[1]/4,3) )
        for chan in range(3):
            zzz[:,:,chan]=scipy.ndimage.interpolation.zoom(imr[:,:,chan], 0.25)
    else:
        zzz = zeros( (imr.shape[0]/1,imr.shape[1]/1,3) )
        for chan in range(3):
            zzz[:,:,chan]=scipy.ndimage.interpolation.zoom(imr[:,:,chan], 1.0)

    ## Intrinsic parameters
    focal_distance = job_params['focal_distance']
    p_point = array(job_params['principal_point'])

    if Downscale:
        focal_distance = focal_distance / 4
        p_point = p_point / 4


    ## Creates picture object
    pic = PicturePinhole(zzz, focal_distance, p_point[0], p_point[1])

    ## Don take last columns in consideration...
    pic.mask[:,-3:] = 0 #remove last column


    ## Run the grid sweep, and generate the edgels list.
    ## Edgel extractor parameters
    gspc = job_params['grid_spacing']
    glim = job_params['gradient_threshold'] # ** -1
    pic.p3 = job_params['angular_threshold'] ## this is for the orientation estimation
    time1=time.clock()
    pic.extract_edgels(gspc, glim)
    time1=time.clock()-time1
    print 'Time sweeping:',time1
    ##
    #################################################################




    if PlotStuff:
        figure(20, figsize=(6,4))
        suptitle('Extracted edgels', fontweight='bold', fontsize=20)
        imshow(0.25+0.75*pic.frame/256., interpolation='nearest', extent=(-.5,1000-.5,750-.5,-.5))

        k = 0.3 * gspc
        for ied in pic.edgels:
            ed = copy(ied)
            ed[0] = ed[0]
            ed[1] = ed[1]
            plot([ed[0] - k * ed[3], ed[0] + k * ed[3]],
                 [ed[1] + k * ed[2], ed[1] - k * ed[2]], '-', lw=0.5, color='#cc0000')

        axis([350,550,220,100])
        savefig('edgel_sample.pdf', dpi=300)
